/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#!/usr/bin/python
# Example of Map server in python

import sys, threading, math, time, random, array
sys.path.append("../../../../GIMI/wrap/Python/lib/")
sys.path.append("../../../interfaces/wrap/Python/lib/")
import MapServer as MS
import gimi


def parseArgs(args):
	parsedArgs = {}
	nextIsValueOf = ""
	for arg in args:
		if not arg:
			continue
		if arg[0] == '-':
			nextIsValueOf = arg[1:]
			parsedArgs[nextIsValueOf] = ""
		else:
			if nextIsValueOf != "":
				parsedArgs[nextIsValueOf] = arg
				nextIsValueOf = ""
	return parsedArgs



def printUsage():
	print """
This is an example of how to use wrapped Map Server with Python.
 
Usage is:	%s [OPTIONS]
  [-u addr]			Use 'addr' as GIMnet AccessPoints address
  [-p port]			Use 'port' as GIMnet AccessPoints port
  [-n name]			Use 'name' as this nodes GIMnet name
  [-G name]			Specify the MaCIGroup for this module
  [-I name]			Specify the MaCIGroup Interface Instance

  
  [-v]				Increase verbosity by 1
  [-q]				Quiet mode
  [-h]				print this
	"""%sys.argv[0]

		
name = "PyMapServerExample"
groupName = "MapServerExample"
instanceName = "suurvisiiri"
server = "asrobo.hut.fi"
port = 40002

args = parseArgs(sys.argv)
print args

if args.has_key("h"):
	printUsage()
	sys.exit()

if args.has_key("u"):server = args['u']
if args.has_key("p"):port = int(args['p'])
if args.has_key("n"):name = args['n']
if args.has_key("G"):groupName = args['G']
if args.has_key("I"):instanceName = args['I']



#MaCI connection
g = gimi.GIMI()
mcs = MS.CMaCICtrlServer(g)
ms = MS.CMapServer(mcs, 0)

mcs.SetGroupName(groupName) 
ms.SetInterfaceInstanceName(instanceName)


print "Connecting to %s:%s as %s..."%(server, port, name), 
ret = g.connectToHubEx(server, port, name)
if ret != MS.KMaCIOK:
	print "Failed: ", MS.GetErrorStr(ret)
	sys.exit()
else: print MS.GetErrorStr(ret)



print "Opening... ",
ret = ms.Open()
if ret != MS.KMaCIOK:
	print "Failed: ", MS.GetErrorStr(ret)
	sys.exit()
else: print MS.GetErrorStr(ret)
del ret
#time.sleep(1.0) # got to sleep to wait for something to get ready to serve us


print "start serving"
#Create data to be served 
d = 0.1
reso=0.1 #grid map resolution
number=0 #block numbering
mapData = MS.CMapData()
mapDataUpdate = MS.CMapData()
mapData.CreateInternalBinBag()
mapDataUpdate.CreateInternalBinBag()
MapHeader = MS.TMapHeader()
MapLine = MS.TMapLine()
UpdateHeader = MS.TMapUpdateHeader()
hmBlock = MS.TGridBlockHeader()
occuBlock = MS.TGridBlockHeader()
ts = MS.TTimestamp()
hmData = []
occuData = []
MapHeader.gridDataResolution=reso

#Serve hungry clients
try:
	while True:
		if number % 100:
			UpdateHeader.clear=False
		else:
			print "Creating new map"
			#clear map on every 100th update
			UpdateHeader.clear=True
			mapData.Reset()
			mapData.SetMapHeader(MapHeader)

		if not mapDataUpdate.SetMapUpdateHeader(UpdateHeader):
			print "Failed to set map update header"
    
		MapLine.x1=random.random()*40-20
		MapLine.y1=random.random()*40-20
		MapLine.x2=random.random()*40-20
		MapLine.y2=random.random()*40-20
    
		if not mapDataUpdate.AddMapLine(MapLine):
			print "Failed to add line to update data"

		if not mapData.AddMapLine(MapLine):
			print "Failed to add line to whole map's data"

		#set block's number
		#occupancy grid and height map are considered separate layers so same numbers can be used
		hmBlock.number=number
		occuBlock.number=number

		#increment block number
		number +=1

		#randomize block location and dimensions
		#TODO: these values must be multiples of the resolution!!
		hmBlock.x=random.random()*50-25
		hmBlock.y=random.random()*50-25
		hmBlock.samples_x=11+int(random.random()*20)
		hmBlock.samples_y=11+int(random.random()*20)

		occuBlock.x=random.random()*50-25
		occuBlock.y=random.random()*50-25
		occuBlock.samples_x=11+int(random.random()*20)
		occuBlock.samples_y=11+int(random.random()*20)

		hmBlock.dataOriginX = 0
		hmBlock.dataOriginY = 0
		occuBlock.dataOriginX=0
		occuBlock.dataOriginY=0
        
		#remember to set data types right
		hmBlock.dataType=MS.KGridTypeHeightMap
		for i in range(hmBlock.samples_x*hmBlock.samples_y): hmData.append(random.random()*10)
		hmBlock.dataSize=hmBlock.samples_x*hmBlock.samples_y*MS.KGridTypeHeightMap #data size in bytes

		occuBlock.dataType=MS.KGridTypeOccupancy
		for i in range(occuBlock.samples_x*occuBlock.samples_y): occuData.append(int(random.random()*100+100))
		occuBlock.dataSize=occuBlock.samples_x*occuBlock.samples_y*MS.KGridTypeOccupancy #data size in bytes

		print "occuData=%u"%occuData[0]


		#draw straight lines to the data to check whether client side drawing works
		for i in range(occuBlock.samples_x/2*occuBlock.samples_y + occuBlock.samples_x/4, 
		occuBlock.samples_x/2*occuBlock.samples_y + 3*occuBlock.samples_x/4):
			occuData[i] = 1

		for i in range(occuBlock.samples_y/4, 3*occuBlock.samples_y/4):
			occuData[i*occuBlock.samples_x + occuBlock.samples_x/2] = 1

		for i in range(hmBlock.samples_x): 
			hmData[hmBlock.samples_y/2*hmBlock.samples_x+i] = 1

		for i in range(hmBlock.samples_y): 
			hmData[i*hmBlock.samples_x+hmBlock.samples_x/2] = 1

#add the grid data to package		

		oo = array.array('B', occuData)
		hh = array.array('f', hmData)
#		print hh
#		print hh.tostring().__str__()
		if not mapData.AddGridBlock(occuBlock, oo.tostring()):
			print "Failed to add occupancy grid block to whole map's data"

		if not mapDataUpdate.AddGridBlock(occuBlock, oo.tostring()):
			print "Failed to add occupancy grid block to update data"

		if not mapData.AddGridBlock(hmBlock, hh.tostring()):
			print "Failed to add height map block to whole map's data"

		if not mapDataUpdate.AddGridBlock(hmBlock, hh.tostring()):
			print "Failed to add height map block to update data"


		bb =  mapDataUpdate.GetGridBlockHeader(0)
		if bb:
			print "sending hm block: x=%f y=%f w=%d h=%d ox=%d oy=%d"%(bb.x, bb.y,
			bb.samples_x, bb.samples_y, bb.dataOriginX, bb.dataOriginY)
		else:
			print bb.__str__()

		bb =  mapDataUpdate.GetGridBlockHeader(1)
		if bb:
			print "sending occu block: x=%f y=%f w=%d h=%d ox=%d oy=%d"%(bb.x, bb.y,
			bb.samples_x, bb.samples_y, bb.dataOriginX, bb.dataOriginY)
		else:
			print bb.__str__()

		area = MS.TArea()
		area.name = "area %d"%number
		a_x=random.random()*50-25
		a_y=random.random()*50-25
		area.vertices.push_back(MS.TVertex(a_x-1,a_y-1))
		area.vertices.push_back(MS.TVertex(a_x+1,a_y-1))
		area.vertices.push_back(MS.TVertex(a_x+1,a_y+1))
		area.vertices.push_back(MS.TVertex(a_x-1,a_y+1))

		if not mapDataUpdate.AddArea(area):
			print "Failed to add area to update data"

		if not mapData.AddArea(area):
			print "Failed to add area to whole map's data"

		ms.SetMap(mapData)

		if ms.SendMapUpdateEvent(mapDataUpdate):
			print "Sent map update"
			mapDataUpdate.Print(2)

		mapDataUpdate.Reset()

		hmData = []
		occuData = []

		time.sleep(3.000)

except KeyboardInterrupt:
	print "\nClosing %s"%name
ms.Close()




#import sys, time
#sys.path.append("../../../interfaces/wrap/Python/lib")
#sys.path.append("../../../../GIMI/wrap/Python/lib")
#import MapClient as MC
#import gimi
#g = gimi.GIMI()
#mapClient = MC.CMapClient(g,0)
#ret = g.connectToHubEx("asrobo.hut.fi",40002,"")
#print MC.GetErrorStr(ret)
#mapClient.SetDefaultTarget(MC.SMaCISL("hinkuvinku.MaCI_Map.suurvisiiri"))
#ret = mapClient.Open();
#if ret == MC.KMaCIOK:
#    print "Succesfully opened map client"
#else:
#    print "Failed to open map client"

#mapData = MC.CMapData()


